#!/usr/bin/env python3
# coding:utf-8
import time
import sys
import os
path=os.path.abspath(".")
sys.path.insert(0,"/home/orin/wurenzhengfeng_ws/src/TFLuna_i2c/scripts")
sys.path.insert(0,"/home/orin/wurenzhengfeng_ws/src/TFLuna_i2c/msg")
import tfli2c as tfl    
import rospy
from TFLuna_i2c.msg import RadarAvoidance
tflAddr1=0x10 
tflAddr2=0x20 
tflAddr3=0x30 
tflPort=7
WarnDis1=300
WarnDis2=300 
WarnDis3=300
lastDirection=6
avoidCounter=0
if __name__ == '__main__':
    # 创建节点
    rospy.init_node("RadarAvoidance")
    pub=rospy.Publisher("/radar_avoidance_data",RadarAvoidance,queue_size=10)
    AvoidData=RadarAvoidance()
    while not tfl.begin(tflAddr1,tflPort):
        rospy.logerr("TFLuna1 lost")
    while not tfl.begin(tflAddr2,tflPort):
        rospy.logerr("TFLuna2 lost") 
    while not tfl.begin(tflAddr3,tflPort):
        rospy.logerr("TFLuna3 lost") 
    rate=rospy.Rate(50)
    while not rospy.is_shutdown():
        disl=tfl.getData(tflAddr1,tflPort)
        disc=tfl.getData(tflAddr2,tflPort)
        disr=tfl.getData(tflAddr3,tflPort)

        if((disl<WarnDis1) and (disc<WarnDis2) and (disr<WarnDis3)):
            AvoidData.direction=0
            AvoidData.avoidance_flag=True
        elif((disl<WarnDis1) and (disr>WarnDis3)):
            AvoidData.direction=5
            AvoidData.avoidance_flag=True
        elif((disl>WarnDis1) and (disr<WarnDis3)):
            AvoidData.direction=4
            AvoidData.avoidance_flag=True
        elif((disl>WarnDis1) and (disc<WarnDis2) and (disr>WarnDis3)):
            AvoidData.direction=0
            AvoidData.avoidance_flag=True
        elif((disl>WarnDis1) and (disc>WarnDis2) and (disr>WarnDis3)):
            AvoidData.direction=0
            AvoidData.avoidance_flag=False
        elif((disl<WarnDis1) and (disc>WarnDis2) and (disr<WarnDis3)):
            AvoidData.direction=0
            AvoidData.avoidance_flag=False
        if(AvoidData.avoidance_flag):
            avoidCounter=avoidCounter+1
        else:
            avoidCounter=0
        if(avoidCounter>500):
            AvoidData.avoidance_flag=False
            AvoidData.ladar_flag=False
        print(disl,disc,disr)
        print('\n')
        pub.publish(AvoidData)
        rate.sleep()    	
    	
    
    

